package flca.voicenav.util;

import java.util.List;

import no.geosoft.cc.geometry.Geometry;

import android.util.Log;

import com.google.android.maps.GeoPoint;

import flca.voicenav.VoiceNavConstants;
import flca.voicenav.model.GpxRoute;
import flca.voicenav.model.TracePoints;
import flca.voicenav.model.TrkPt;

public class DistHelper implements VoiceNavConstants
{

	/**
	 * Returns closest point on segment to point
	 * 
	 * @param ss
	 *            segment start point
	 * @param se
	 *            segment end point
	 * @param p
	 *            point to found closest point on segment
	 * @return closest point on segment to p
	 */
	public static Point getClosestPointOnSegment(Point ss, Point se, Point p)
	{
		return getClosestPointOnSegment(ss.x, ss.y, se.x, se.y, p.x, p.y);
	}

	public static GeoPoint getClosestPointOnSegment(GeoPoint p1, GeoPoint p2, GeoPoint testPnt) 
	{
		Point r = getClosestPointOnSegment(p1.getLatitudeE6(), p1.getLongitudeE6(), 
				p2.getLatitudeE6(), p2.getLongitudeE6(), testPnt.getLatitudeE6(), testPnt.getLongitudeE6());
		return new GeoPoint(r.x, r.y);
	}

	/**
	 * Returns closest point on segment to point
	 * 
	 * @param sx1
	 *            segment x coord 1
	 * @param sy1
	 *            segment y coord 1
	 * @param sx2
	 *            segment x coord 2
	 * @param sy2
	 *            segment y coord 2
	 * @param px
	 *            point x coord
	 * @param py
	 *            point y coord
	 * @return closets point on segment to point
	 */
	public static Point getClosestPointOnSegment(int sx1, int sy1, int sx2, int sy2, int px, int py)
	{
		double xDelta = sx2 - sx1;
		double yDelta = sy2 - sy1;

		if((xDelta == 0) && (yDelta == 0))
		{
//			throw new IllegalArgumentException("Segment start equals segment end");
			return new Point(sx1, sy1);
		}

		double u = ((px - sx1) * xDelta + (py - sy1) * yDelta) / (xDelta * xDelta + yDelta * yDelta);

		final Point closestPoint;
		if(u < 0)
		{
			closestPoint = new Point(sx1, sy1);
		}
		else if(u > 1)
		{
			closestPoint = new Point(sx2, sy2);
		}
		else
		{
			closestPoint = new Point((int) Math.round(sx1 + u * xDelta), (int) Math.round(sy1 + u * yDelta));
		}

		return closestPoint;
	}
	
	public static boolean isPointOnSegment(GeoPoint p1, GeoPoint p2, GeoPoint testPnt) 
	{
		GeoPoint projPt = getClosestPointOnSegment(p1, p2, testPnt);
		double d1 = getDistance(p1, projPt);
		double d2 = getDistance(p2, projPt);
		
		if (d1 == 0.0 || d2 == 0) {
			return isOrthogonalOnEndpoint(p1, p2, testPnt);
		} else {
			return true;
		}
	}

	private static boolean isOrthogonalOnEndpoint(GeoPoint p1, GeoPoint p2, GeoPoint testPnt) 
	{
		double d1 = getDistance(p1, testPnt);
		double d2 = getDistance(p2, testPnt);
		double d3 = getDistance(p1, p2);
		
		if (d1 > d2) {
			return Math.pow(d1, 2.0) == Math.pow(d2, 2.0) + Math.pow(d3, 2.0);
		} else {
			return Math.pow(d2, 2.0) == Math.pow(d1, 2.0) + Math.pow(d3, 2.0);
		}
	}

	/**
	 * Returns distance to segment
	 * 
	 * @param ss
	 *            segment start point
	 * @param se
	 *            segment end point
	 * @param p
	 *            point to found closest point on segment
	 * @return distance to segment
	 */
	public static double getDistanceToSegment(Point ss, Point se, Point p)
	{
		return getDistanceToSegment(ss.x, ss.y, se.x, se.y, p.x, p.y);
	}

	/**
	 * Returns distance to segment
	 * 
	 * @param sx1 segment x coord 1
	 * @param sy1 segment y coord 1
	 * @param sx2 segment x coord 2
	 * @param sy2 segment y coord 2
	 * @param px point x coord
	 * @param py point y coord
	 * @return distance to segment
	 */
	public static double getDistanceToSegment(int sx1, int sy1, int sx2, int sy2, int px, int py)
	{
		Point closestPoint = getClosestPointOnSegment(sx1, sy1, sx2, sy2, px, py);
		return getDistance(closestPoint.x, closestPoint.y, px, py);
	}

	public static double getDistanceToSegment(TrkPt segmp1, TrkPt segmp2, GeoPoint p)
	{
		GeoPoint closestPoint = getClosestPointOnSegment(segmp1.toGeoPoint(), segmp2.toGeoPoint(), p);
		return getDistance(closestPoint, p);
	}

	public static double getDistanceToSegment(GeoPoint segmp1, GeoPoint segmp2, GeoPoint p)
	{
		GeoPoint closestPoint = getClosestPointOnSegment(segmp1, segmp2, p);
		return getDistance(closestPoint, p);
	}

	/**
	 * Returns distance between two 2D points
	 * 
	 * @param point1
	 *            first point
	 * @param point2
	 *            second point
	 * @return distance between points
	 */
	public static double getDistance(GeoPoint point1, GeoPoint point2) {
		return calcDistance((point1.getLatitudeE6()/1E6), (point1.getLongitudeE6()/1E6), 
				 (point2.getLatitudeE6()/1E6), (point2.getLongitudeE6()/1E6));
	}

	/**
	 * Returns distance between two sets of coords
	 * 
	 * @param x1
	 *            first x coord
	 * @param y1
	 *            first y coord
	 * @param x2
	 *            second x coord
	 * @param y2
	 *            second y coord
	 * @return distance between sets of coords
	 */
	public static double getDistance(float x1, float y1, float x2, float y2) 
	{
		return calcDistance(x1, y1, x2, y2);
	}

//	public static double getDistance(double x1, double y1, double x2, double y2) 
//	{
//		return calcDistance(x1, y1, x2, y2);
//	}
	
	public static double calcDistance(TrkPt p1, TrkPt p2) 
	{
		return calcDistance(p1.toGeoPoint(), p2.toGeoPoint());
	}

	public static double calcDistance(double lat1, double lng1, double lat2, double lng2) 
	{
		GeoPoint latlng1 = makeGeoPoint(lat1, lng1);	
		GeoPoint latlng2 = makeGeoPoint(lat2, lng2);	
		return calcDistance(latlng1, latlng2);
	}

	private static GeoPoint makeGeoPoint(double lat1, double lng1) {
		return new GeoPoint((int) (lat1 * 1E6), (int) (lng1 * 1E6));	
	}
	
	public static double calcDistance(GeoPoint latlng1, GeoPoint latlng2) 
	{
		double r = distance(latlng1, latlng2, LengthUnit.METER);
		return r;
	}
	
	public static double calcAngle(GeoPoint p1, GeoPoint p2, GeoPoint p3)
	{
		Line2D line1 = new Line2D(p1, p2);
		Line2D line2 = new Line2D(p2, p3);
        return calcAngle(line1, line2);
	}

	public static double calcAngle(Line2D line1, Line2D line2)
	{
        double angle1 = Math.atan2(line1.getY1() - line1.getY2(),
                                   line1.getX1() - line1.getX2());
        double angle2 = Math.atan2(line2.getY1() - line2.getY2(),
                                   line2.getX1() - line2.getX2());
        double r = Math.toDegrees(angle1-angle2);

        return r;
	}

	

	/**
	 * Distance between two points.
	 * 
	 * @param point1 the first point.
	 * @param point2 the second point.
	 * @param unit the unit of measure in which to receive the result.
	 * @return the distance in the chosen unit of measure.
	 */
	public static double distance(GeoPoint point1, GeoPoint point2, LengthUnit unit) {
		return distanceInRadians(point1, point2)
				* LatLngConfig.getEarthRadius(unit);
	}

	/**
	 * <p>This "distance" function is mostly for internal use. Most users will simply
	 * rely upon {@link #distance(GeoPoint, GeoPoint, LengthUnit)}</p>
	 * 
	 * <p>Yields the internal angle for an arc between two points on the surface of a sphere
	 * in radians. This angle is in the plane of the great circle connecting the two points
	 * measured from an axis through one of the points and the center of the Earth.
	 * Multiply this value by the sphere's radius to get the length of the arc.</p>
	 * 
	 * @return the internal angle for the arc connecting the two points in radians.
	 */
	public static double distanceInRadians(GeoPoint point1, GeoPoint point2) {
		double lat1R = Math.toRadians(getLat(point1));
		double lat2R = Math.toRadians(getLat(point2));
		double dLatR = Math.abs(lat2R - lat1R);
		double dLngR = Math.abs(Math.toRadians(getLng(point2) - getLng(point1)));
		double a = Math.sin(dLatR / 2) * Math.sin(dLatR / 2) + Math.cos(lat1R)
				* Math.cos(lat2R) * Math.sin(dLngR / 2) * Math.sin(dLngR / 2);
		return 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
	}

	private static double getLat(GeoPoint aGeoPoint) {
		return (double) (aGeoPoint.getLatitudeE6() / 1E6);
	}
	private static double getLng(GeoPoint aGeoPoint) {
		return (double) (aGeoPoint.getLongitudeE6() / 1E6);
	}

	/**
	 * Clamp latitude to +/- 90 degrees.
	 * 
	 * @param latitude in degrees.
	 * @return the normalized latitude. Returns NaN if 
	 * the input is NaN.
	 */
	public static double normalizeLatitude(double latitude) {
		if (Double.isNaN(latitude))
			return Double.NaN;
		if (latitude > 0) {
			return Math.min(latitude, 90.0);
		} else {
			return Math.max(latitude, -90.0);
		}
	}

	/**
	 * Convert longitude to be within the +/- 180 degrees range.
	 * 
	 * @param longitude in degrees.
	 * @return the normalized longitude. Returns NaN if the input
	 * is NaN, positive infinity, or negative infinity.
	 */
	public static double normalizeLongitude(double longitude) {
		if (Double.isNaN(longitude) || Double.isInfinite(longitude))
			return Double.NaN;
		double longitudeResult = longitude % 360;
		if (longitudeResult > 180) {
			double diff = longitudeResult - 180;
			longitudeResult = -180 + diff;
		} else if (longitudeResult < -180) {
			double diff = longitudeResult + 180;
			longitudeResult = 180 + diff;
		}
		return longitudeResult;
	}

	public static double calcTotalDistance(List<GeoPoint> aPoints, LengthUnit aUnit)
	{
		double dist = 0.0;
		
		for (int i = 1; i < aPoints.size(); i++) {
			dist = dist + distance(aPoints.get(i-1), aPoints.get(i), aUnit);
		}
		
		return dist;
	}

	public static double calcSpeed(TracePoints aTracePoints) 
	{
		double result = 0.0;
		
		result = calcSpeedUsingLocationSpeed(aTracePoints);
		if (result == 0.0) { // hasSpeed() is false;
			Log.w(TAG, "speed is unknow"); //TODO
		}
		
		return result;
	}

	private static double calcSpeedUsingLocationSpeed(TracePoints aTracePoints)
	{
		double result;
		double speed1 = 0.0, speed2, speed3;
		if (aTracePoints.size() > 0) {
			speed1 = aTracePoints.get(aTracePoints.size()-1).getLocation().getSpeed();
		} 
		if (aTracePoints.size() > 1) {
			speed2 = aTracePoints.get(aTracePoints.size()-2).getLocation().getSpeed();
		} else {
			speed2 = speed1;
		}
		if (aTracePoints.size() > 2) {
			speed3 = aTracePoints.get(aTracePoints.size()-3).getLocation().getSpeed();
		} else {
			speed3 = speed2;
		}
		
		result = 3*speed1 + 2*speed2 + speed3 / 6.0;
		return result;
	}
	
	
	public static int calcMinDistance(GeoPoint aLocation, GpxRoute aRoute) 
	{
		int result = 1000000;
		
		for (int i=1; i < aRoute.getCoordinates().size(); i++) {
			TrkPt p1 = aRoute.getCoordinates().get(i-1);
			TrkPt p2 = aRoute.getCoordinates().get(i);
			double d = getDistanceToSegment(p1, p2, aLocation);
			if ((int) d < result) {
				result = (int) d;
			}
		}
		return result;
	}
	
	//---- static helper classes ---------------------
	public static class Point {
		public int x;
		public int y;
		
		public Point(int x, int y) {
			this.x = x;
			this.y = y;
		}
	}

	public static class Line2D {
		
		private int x1, y1, x2, y2;

		public Line2D(GeoPoint p1, GeoPoint p2) {
			this(p1.getLatitudeE6(), p1.getLongitudeE6(), p2.getLatitudeE6(), p2.getLongitudeE6());
		}
		
		public Line2D(int x1, int y1, int x2, int y2) {
			super();
			this.x1 = x1;
			this.y1 = y1;
			this.x2 = x2;
			this.y2 = y2;
		}

		public int getX1() {
			return x1;
		}

		public void setX1(int x1) {
			this.x1 = x1;
		}

		public int getY1() {
			return y1;
		}

		public void setY1(int y1) {
			this.y1 = y1;
		}

		public int getX2() {
			return x2;
		}

		public void setX2(int x2) {
			this.x2 = x2;
		}

		public int getY2() {
			return y2;
		}

		public void setY2(int y2) {
			this.y2 = y2;
		}
	}
	
}
